#ifndef _serial_protocol_hpp
#define _serial_protocol_hpp

#include "ros/ros.h"
#include "vector"

namespace SerialProtocol
{

#define SERIAL_PROTOCOL_HEAD1 0Xff
#define SERIAL_PROTOCOL_HEAD2 0Xff


typedef enum
{
    WAIT_HD1,
    WAIT_HD2,
    WAIT_RANDOM1,
    WAIT_RANDOM2,
    WAIT_CRC1,
    CRC_CHECK,
    HD_READY
}header_unpack_state;

typedef enum
{
    WAIT_HD_READY,
    GET_DATA_LEN,
    GET_MSG_ID,
    GET_DATA,
    WAIR_CRC1,
    WAIT_CHECKALL,
    DATA_READY
}data_unpack_state;


uint16_t crc16_cal_func(uint8_t *buf, uint8_t len)
{
    uint16_t crc = 0xFFFF;

    for (uint8_t i = 0; i < len; i++)
    {
        crc = crc ^ buf[i];

        for (uint8_t j = 0; j < 8; j++)
        {
            uint8_t chk;

            chk = crc & 1;
            crc = (crc >> 1) & 0x7fff ;
            if (chk == 1)
                crc = crc ^ 0xa001;
            crc = crc & 0xffff;
        }
    }

    return crc;
}

//数据头校验类
class header_unpack
{
public:
    uint8_t random[2];
    uint8_t crc16[2];
    header_unpack_state header_state;
    
    bool header_check(uint8_t *crc16)
    {
        uint16_t cal_crc16;
        uint8_t cal_cal16_high;
        uint8_t cal_cal16_low;
        uint8_t header_data[4];

        header_data[0] = SERIAL_PROTOCOL_HEAD1;
        header_data[1] = SERIAL_PROTOCOL_HEAD2;
        header_data[2] = random[0];
        header_data[3] = random[1];

        cal_crc16 = crc16_cal_func(header_data, 4);

        cal_cal16_high = cal_crc16 >> 8;
        cal_cal16_low = cal_crc16 & 0xff;

        static int cnt_header_ok = 0;
        static int cnt_header_error = 0;

        if(cal_cal16_low == crc16[0] && cal_cal16_high == crc16[1])
        {
            cnt_header_ok++;
            // printf("cnt hd ok = %d  cnt hd error = %d\r\n", cnt_header_ok, cnt_header_error);
            return true;
        }
        else 
        {
            cnt_header_error++;
            // printf("cnt hd ok = %d  cnt hd error = %d\r\n", cnt_header_ok, cnt_header_error);
            return false;
        }
    }

    header_unpack_state push(uint8_t newbyte) // 数据头获取状态机
    {
        switch (header_state)
        {
            case WAIT_HD1:
                if(newbyte == SERIAL_PROTOCOL_HEAD1){
                    header_state = WAIT_HD2;
                }
                break;

            case WAIT_HD2:
                if(newbyte == SERIAL_PROTOCOL_HEAD2){
                    header_state = WAIT_RANDOM1;
                }
                break;

            case WAIT_RANDOM1:
                random[0] = newbyte;
                header_state = WAIT_RANDOM2;
                break;

            case WAIT_RANDOM2:
                random[1] = newbyte;
                header_state = WAIT_CRC1;
                break;

            case WAIT_CRC1:
                crc16[0] = newbyte;
                header_state = CRC_CHECK;
                break;

            case CRC_CHECK:
                crc16[1] = newbyte;
                static int cnt_pack_ok = 0;
                static int cnt_pack_error = 0;

                if( header_check(crc16) ){
                    header_state = HD_READY;
                    cnt_pack_ok++;
                }else{
                    header_state = WAIT_HD1;
                    cnt_pack_error++;
                }

               // printf("cnt pack ok = %d  cnt pack error = %d\r\n", cnt_pack_ok, cnt_pack_error);

                break;

            case HD_READY:  // header获取 与 data获取,并行完成.
                if(newbyte == SERIAL_PROTOCOL_HEAD1){
                    header_state = WAIT_HD2;
                }
                else{
                    header_state = WAIT_HD1;
                }
                break;
                
            default:
                header_state = WAIT_HD1;
                break;
        }
        return header_state;
    }

};


//数据获取类
class data_unpack
{
private:
    header_unpack hd;
    uint8_t crc16[2];
    uint8_t data_len;
    uint8_t msgID;
    std::vector<uint8_t> data;
    data_unpack_state unpack_state;

    // This function only checks the data part but ignore the header part
    bool data_check(uint8_t * crc16){
        uint16_t cal_crc16;
        uint8_t cal_cal16_high;
        uint8_t cal_cal16_low;
        uint8_t full_data_len = data_len + 2; // data_len, msgID, data
        uint8_t* full_data = new uint8_t[full_data_len];

        full_data[0] = data_len;
        full_data[1] = msgID;

        for(int i = 0; i < data_len; i++){ 
            full_data[i+2] = data[i];
        }

        cal_crc16 = crc16_cal_func(full_data, full_data_len);

        delete[] full_data; //内存释放

        cal_cal16_high = cal_crc16 / 256;
        cal_cal16_low = cal_crc16 % 256;

        if(cal_cal16_low == crc16[0] && cal_cal16_high == crc16[1]){
            return true;
        }
        else {
            return false;
        }
    }
    

    void reset(void)
    {
        data.clear();
        unpack_state = WAIT_HD_READY;
    }

public:
    std::vector<uint8_t> getData()
    {
        return data;
    }

    uint8_t getDataLen()
    {
        return data_len;
    }

    uint8_t getID()
    {
        return msgID;
    }

    data_unpack_state push(uint8_t newbyte) //数据头及数据区获取状态机
    {
        if(hd.push(newbyte) == HD_READY)
        {
            data.clear();
            unpack_state = GET_DATA_LEN;
            // ROS_INFO("WAIR_CRC1");
        }
        else
        {
            switch (unpack_state)
            {
                case WAIT_HD_READY:
                    // ROS_INFO("WAIT_HD_READY");
                    break;
                    
                case GET_DATA_LEN:
                    data_len = newbyte;
                    unpack_state = GET_MSG_ID;
                    break;
                
                case GET_MSG_ID:
                    msgID = newbyte;
                    unpack_state = GET_DATA;
                    break;

                case GET_DATA:  //获取数据
                    data.push_back(newbyte);
                    if(data.size() >= data_len)  
                    {
                        unpack_state = WAIR_CRC1;
                    }
                    break;

                case WAIR_CRC1:
                    crc16[0] = newbyte;
                    unpack_state = WAIT_CHECKALL;
                    break;

                case WAIT_CHECKALL:
                    crc16[1] = newbyte;
                    if(data_check(crc16))
                    {
                        unpack_state = DATA_READY;
                        // ROS_INFO("DATA_READY");
                    }else
                    {
                        reset();
                    }
                    break;
                    
                case DATA_READY:
                    reset();

                default:
                    reset();
                    break;
            }
        }
        return unpack_state;
    }
};


class data_pack
{
private:
    uint16_t random = 0x0000;
    uint16_t len;

public:
    std::vector<uint8_t> packbuff;


    void set_header(uint8_t set_len, uint8_t set_id)
    {
        len = set_len;
        
        random += 1;

        packbuff.clear();
        packbuff.resize(len + 10);

        packbuff[0] = SERIAL_PROTOCOL_HEAD1;
        packbuff[1] = SERIAL_PROTOCOL_HEAD2;
        packbuff[2] = random & 0xff; //低8位
        packbuff[3] = random >> 8; //高8位
        
        uint16_t crc16_header = crc16_cal_func(&packbuff[0], 4);//header CRC
        packbuff[4] = crc16_header & 0xff; 
        packbuff[5] = crc16_header >> 8; 

        packbuff[6] = set_len;
        packbuff[7] = set_id;
    }

    bool set_buff(std::vector<uint8_t> &databuff)
    {
        if(databuff.size() != len)
        {
            return false;
        }
        else
        {
            for(int i = 0; i <= len - 1; i++)
            {
                packbuff[8 + i] = databuff[i];
            }            
            return true;
        }

    }

    void set_crc(void)
    {
        uint16_t crc16 = crc16_cal_func(&packbuff[6], len + 2);
        packbuff[len + 8] = crc16 & 0xff; //低8位
        packbuff[len + 9] = crc16 >> 8; //高8位
    }


};

}

#endif
